// Camera.cpp
#include "camera.h"
 
#include <iostream>
#include <libobsensor/hpp/Context.hpp>
#include <libobsensor/hpp/Device.hpp>
#include <libobsensor/hpp/Error.hpp>
#include <libobsensor/hpp/StreamProfile.hpp>
#include <memory>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
 
constexpr bool FLIP = false;

// 新增
#include "utils.hpp"
// 新增
void Camera::pressKeyExit(std::string msg) {
    if(msg.size() > 0) {
        std::cout << msg << std::endl;
    }
    while(true) {
        // Get the value of the pressed key, if it is the esc key, exit the program
        int key = getch();
        if(key == ESC)
            break;
    }
}
// Convert depth precision unit to 1mm
void Camera::depthPrecisionConversion(std::shared_ptr<ob::DepthFrame> depthFrame) {
    auto size  = depthFrame->dataSize();
    auto pixel = static_cast<uint16_t *>(depthFrame->data());

    for(int i = 0; i < size / sizeof(uint16_t); i++) {
        pixel[i] = static_cast<uint16_t>(pixel[i] * unit);
    }
}

void Camera::init_color() {
    // 获取彩色相机的所有流配置，包括流的分辨率，帧率，以及帧的格式
    auto profiles = this->pipe->getStreamProfileList(OB_SENSOR_COLOR);
    std::shared_ptr<ob::VideoStreamProfile> color_profile = nullptr;
    try {
        // 根据指定的格式查找对应的Profile,优先选择RGB888格式
        color_profile = profiles->getVideoStreamProfile(640, 0, OB_FORMAT_RGB888, 30);
    } catch (const ob::Error &) {
        // 没找到RGB888格式后不匹配格式查找对应的Profile进行开流
        color_profile = profiles->getVideoStreamProfile(640, 0, OB_FORMAT_UNKNOWN, 30);
    }
    // 开启/关闭彩色相机的镜像模式
    if (this->device->isPropertySupported(OB_PROP_COLOR_MIRROR_BOOL, OB_PERMISSION_WRITE)) {
        this->device->setBoolProperty(OB_PROP_COLOR_MIRROR_BOOL, FLIP);
    }
    // 开启彩色流
    this->config->enableStream(color_profile);
}

// 新增
int16_t Camera::init_depth_precision() {
    // Get the current D2D mode
    if(!device->isPropertySupported(OB_PROP_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE)) {
        pressKeyExit("Current device not support disparity to depth !");
        return -1;
    }
    // Get the current D2D mode
    auto curD2dMode = device->getBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL);
    if(!curD2dMode) {
        std::cout << "Current Disparity to depth is software-D2d" << std::endl;
    }
    else {
        std::cout << "Current Disparity to depth is hardware-D2d" << std::endl;
    }

    // Get the current depthPrecisionLevel
    if(!device->isPropertySupported(OB_PROP_DEPTH_PRECISION_LEVEL_INT, OB_PERMISSION_READ_WRITE)) {
        pressKeyExit("Current device not support depthPrecision !");
        return -1;
    }
    curDepthPrecisionlevel = (OBDepthPrecisionLevel)device->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT);
    std::cout << "Current depthPrecision is " << depthUnitMap.find(curDepthPrecisionlevel)->second << std::endl;

    // Get the depthPrecisionList
    if(!device->isPropertySupported(OB_STRUCT_DEPTH_PRECISION_SUPPORT_LIST, OB_PERMISSION_READ) && curD2dMode) {
        pressKeyExit("Current device not support depthPrecisionList !");
        return -1;
    }
    auto depthPrecision = device->getStructuredDataExt(OB_STRUCT_DEPTH_PRECISION_SUPPORT_LIST);
    precisionList.clear();
    // Some of the products do not have the same precision list for hardware and software
    if(curD2dMode) {
        int count = 0;
        for(int i = 0; i < depthPrecision->dataSize; i += 2) {
            auto precision = *((uint16_t *)((uint8_t *)depthPrecision->data + i));
            precisionList.push_back(precision);
            std::cout << "precisionList[" << count << "]: " << depthUnitMap.find((OBDepthPrecisionLevel)precisionList[count])->second << std::endl;
            count++;
        }
    }
    else {
        for(int i = 0; i < 6; i++) {
            precisionList.push_back(i);
            std::cout << "precisionList[" << i << "]: " << depthUnitMap.find((OBDepthPrecisionLevel)precisionList[i])->second << std::endl;
        }
    }

    // Let the user choose a depthPrecision, then switch
    if(precisionList.size() > 0) {
        uint32_t index = 0;
        std::cout << "Please input the index from above depthPrecisionList, newIndex = ";
        // std::cin >> index;
        index = 5;
        if(index >= 0 && index < precisionList.size()) {  // legitimacy check
            device->setIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT, index);

            // Checking depth accuracy after switching
            curDepthPrecisionlevel = (OBDepthPrecisionLevel)device->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT);
            std::cout << "Current depthPrecision is " << depthUnitMap.find(curDepthPrecisionlevel)->second << std::endl;
        }
        else {
            std::cout << "switchDepthPrecision faild. invalid index: " << index << std::endl;
        }
    }
    if(curDepthPrecisionlevel != OB_PRECISION_UNKNOWN) {
        switch(curDepthPrecisionlevel) {
        case OB_PRECISION_1MM:
            unit = 1;
            break;
        case OB_PRECISION_0MM8:
            unit = 0.8;
            break;
        case OB_PRECISION_0MM4:
            unit = 0.4;
            break;
        case OB_PRECISION_0MM1:
            unit = 0.1;
            break;
        case OB_PRECISION_0MM2:
            unit = 0.2;
            break;
        case OB_PRECISION_0MM5:
            unit = 0.5;
            break;
        case OB_PRECISION_0MM05:
            unit = 0.05;
            break;
        default:
            unit = -1;
            break;
        }
    }

    device->setBoolProperty(OB_PROP_LDP_BOOL, false);
    device->setBoolProperty(OB_PROP_LASER_BOOL, true);

    return 0;
}

void Camera::init_depth() {
    auto profiles = this->pipe->getStreamProfileList(OB_SENSOR_DEPTH);
    std::shared_ptr<ob::VideoStreamProfile> depth_profile = nullptr;
    try {
        // 根据指定的格式查找对应的Profile,优先查找Y16格式
        depth_profile = profiles->getVideoStreamProfile(640, 0, OB_FORMAT_Y16, 30);
    } catch (const ob::Error &) {
        // 没找到Y16格式后不匹配格式查找对应的Profile进行开流
        depth_profile = profiles->getVideoStreamProfile(640, 0, OB_FORMAT_UNKNOWN, 30);
    }
    // 开启/关闭深度相机的镜像模式
    if (this->device->isPropertySupported(OB_PROP_DEPTH_MIRROR_BOOL, OB_PERMISSION_WRITE)) {
        this->device->setBoolProperty(OB_PROP_DEPTH_MIRROR_BOOL, FLIP);
    }
    // 开启深度流
    this->config->enableStream(depth_profile);
}
 
void Camera::init_IR() {
    // 获取红外相机的所有流配置，包括流的分辨率，帧率，以及帧的格式
    auto profiles = pipe->getStreamProfileList(OB_SENSOR_IR);
    std::shared_ptr<ob::VideoStreamProfile> ir_profile = nullptr;
    try {
        // 根据指定的格式查找对应的Profile,优先查找Y16格式
        ir_profile = profiles->getVideoStreamProfile(640, 0, OB_FORMAT_Y16, 30);
    } catch (const ob::Error &) {
        // 没找到Y16格式后不匹配格式查找对应的Profile进行开流
        ir_profile = profiles->getVideoStreamProfile(640, 0, OB_FORMAT_UNKNOWN, 30);
    }
    // 开启/关闭红外相机的镜像模式
    if (this->device->isPropertySupported(OB_PROP_IR_MIRROR_BOOL, OB_PERMISSION_WRITE)) {
        this->device->setBoolProperty(OB_PROP_IR_MIRROR_BOOL, FLIP);
    }
    // 开启红外流
    this->config->enableStream(ir_profile);
}
 
Camera::Camera(bool color, bool IR, bool depth)
    : pipe(std::make_shared<ob::Pipeline>()), config(std::make_shared<ob::Config>()) {
    // 新增
    this->depthUnitMap = {
        { OB_PRECISION_1MM, "1.0mm" },  { OB_PRECISION_0MM8, "0.8mm" }, { OB_PRECISION_0MM4, "0.4mm" },   { OB_PRECISION_0MM1, "0.1mm" },
        { OB_PRECISION_0MM2, "0.2mm" }, { OB_PRECISION_0MM5, "0.5mm" }, { OB_PRECISION_0MM05, "0.05mm" },
    };
    this->curDepthPrecisionlevel = OB_PRECISION_UNKNOWN;
    this->unit = -1;

    this->device = pipe->getDevice();
    if (color) {
        this->init_color();
    }
    if (IR) {
        this->init_IR();
    }
    if (depth) {
        this->init_depth_precision();
        this->init_depth();
    }
    // 若摄像头支持多种流，则开启流同步
    if (color + IR + depth > 1) {
        if (device->isPropertySupported(OB_PROP_DEPTH_ALIGN_HARDWARE_BOOL, OB_PERMISSION_READ)) {
            config->setAlignMode(ALIGN_D2C_HW_MODE);
        } else {
            config->setAlignMode(ALIGN_D2C_SW_MODE);
        }
    }
    // 如果有新增设备，则开启流；如果有设备拔出，则关闭流。
    ob::Context ctx;
    ctx.setDeviceChangedCallback([this](std::shared_ptr<ob::DeviceList> removed_devices,
                                        std::shared_ptr<ob::DeviceList> added_devices) {
        if (added_devices->deviceCount() > 0) {
            this->pipe = std::make_shared<ob::Pipeline>();
            this->start();
        } else if (removed_devices->deviceCount() > 0) {
            this->stop();
        }
    });
}
 
cv::Mat Camera::frame2mat(const std::shared_ptr<ob::VideoFrame> &frame) {
    if (frame == nullptr || frame->dataSize() < 1024) {
        return {};
    }
 
    const OBFrameType frame_type = frame->type();               // 帧类型（彩色/深度/IR）
    const OBFormat frame_format = frame->format();              // 图像格式
    const int frame_height = static_cast<int>(frame->height()); // 图像高度
    const int frame_width = static_cast<int>(frame->width());   // 图像宽度
    void *const frame_data = frame->data();                     // 帧原始数据首地址
    const int data_size = static_cast<int>(frame->dataSize());  // 帧数据大小
 
    cv::Mat result_mat;
 
    if (frame_type == OB_FRAME_COLOR) {
        // Color image
        if (frame_format == OB_FORMAT_MJPG) {
            const cv::Mat raw_mat(1, data_size, CV_8UC1, frame_data);
            result_mat = cv::imdecode(raw_mat, 1);
        } else if (frame_format == OB_FORMAT_NV21) {
            const cv::Mat raw_mat(frame_height * 3 / 2, frame_width, CV_8UC1, frame_data);
            cv::cvtColor(raw_mat, result_mat, cv::COLOR_YUV2BGR_NV21);
        } else if (frame_format == OB_FORMAT_YUYV || frame_format == OB_FORMAT_YUY2) {
            const cv::Mat raw_mat(frame_height, frame_width, CV_8UC2, frame_data);
            cv::cvtColor(raw_mat, result_mat, cv::COLOR_YUV2BGR_YUY2);
        } else if (frame_format == OB_FORMAT_RGB888) {
            const cv::Mat raw_mat(frame_height, frame_width, CV_8UC3, frame_data);
            cv::cvtColor(raw_mat, result_mat, cv::COLOR_RGB2BGR);
        } else if (frame_format == OB_FORMAT_UYVY) {
            const cv::Mat raw_mat(frame_height, frame_width, CV_8UC2, frame_data);
            cv::cvtColor(raw_mat, result_mat, cv::COLOR_YUV2BGR_UYVY);
        }
    } else if (frame_format == OB_FORMAT_Y16 || frame_format == OB_FORMAT_YUYV ||
               frame_format == OB_FORMAT_YUY2) {
        // IR or depth image
        const cv::Mat raw_mat(frame_height, frame_width, CV_16UC1, frame_data);
        const double scale =
            1 / pow(2, frame->pixelAvailableBitSize() - (frame_type == OB_FRAME_DEPTH ? 10 : 8));
        cv::convertScaleAbs(raw_mat, result_mat, scale);
    } else if (frame_type == OB_FRAME_IR) {
        // IR image
        if (frame_format == OB_FORMAT_Y8) {
            result_mat = cv::Mat(frame_height, frame_width, CV_8UC1, frame_data);
        } else if (frame_format == OB_FORMAT_MJPG) {
            const cv::Mat raw_mat(1, data_size, CV_8UC1, frame_data);
            result_mat = cv::imdecode(raw_mat, 1);
        }
    }
    return result_mat;
}

uint16_t Camera::get_depth(std::shared_ptr<ob::DepthFrame> &depthFrame, std::vector<cv::Point2f> centers) {
    uint16_t centerDistance = 0;
    // For Y16 format depth frame, print the distance of the center pixel every 30 frames
    if(depthFrame->index() % 50 == 0 && depthFrame->format() == OB_FORMAT_Y16) {
        for (auto &center : centers) {
            uint32_t  width  = depthFrame->width();
            uint32_t  height = depthFrame->height();
            uint16_t *data   = (uint16_t *)depthFrame->data();

            // Pixel value multiplied by scale is the actual distance value in millimeters
            centerDistance = data[((int)center.y-1) * width + (int)center.x];
            // Attention: if the distance is 0, it means that the depth camera cannot detect the object（may be out of detection range）
            std::cout << "The image center depth value is " << centerDistance << " mm. " << std::endl;
        }
    }
    return centerDistance;
}